﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Collections.ObjectModel;
using System.Threading;

namespace SwarmMappingAlpha
{
   
    public class RRController
    {
        //Our interface to roboRealm
        private RR_API rr = new RR_API();

        //Stores the width and height of RR for us
        public static Dimension dimension;

        //contains the drone information from roboRealm
        private List<Drones> robots;

        //contains height of world

        //Delegate and events used for updating those who care about the drones from RR
        public delegate void dronesState(List<Drones> dronesList);
        public event dronesState dronesCollectionModified;

        //manages the polling times for RR
        System.Timers.Timer pollTimer;

        /// <summary>
        /// Constructor
        /// </summary>
        public RRController()
        {
            robots = new List<Drones>();
        }

        /// <summary>
        /// Connects to RoboRealm over socket
        /// </summary>
        public void connectToRR()
        {
            if (!rr.connect("localhost"))
            {
                throw new Exception("Couldn't Connect to RoboRealm");
            }
            dimension = rr.getDimension();
            pollTimer = new System.Timers.Timer(500);
            pollTimer.Elapsed += new System.Timers.ElapsedEventHandler(pollTimer_Elapsed);
            pollTimer.Start();
        }

        /// <summary>
        /// call to poll RR - event fired from timer
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void pollTimer_Elapsed(object sender, System.Timers.ElapsedEventArgs e)
        {
            Thread polling = new Thread(new ThreadStart(pollRR));
            polling.Start();
        }


        /// <summary>
        /// Poll RR for new drone info
        /// Compares the data recieved from RR with what we already have, If it's new it fires an event with the new data
        /// </summary>
        public void pollRR()
        {

            //Variables used to store RR data
            List<String> robotCOGXAsStringArray;
            List<String> robotCOGYAsStringArray;
            List<String> robotWidthAsStringArray;
            List<String> robotHeightAsStringArray;
            List<String> robotRasStringArray;
            List<String> robotGasStringArray;
            List<String> robotBasStringArray;

            String robotCOGXAsString;
            String robotCOGYAsString;
            String robotWidthAsString;
            String robotHeightAsString;
            String robotRasString;
            String robotGasString;
            String robotBasString;

            //flag for changes in drones list
            bool dirtyFlagForRobotsList = false;

            //Extra list for comparison of drones
            List<Drones> checkListForRRDrones = new List<Drones>();

            try
            {

                //Ask RoboRealm for drone data
                robotCOGXAsString = rr.getVariable("COG_X");
                robotCOGYAsString = rr.getVariable("COG_Y");
                robotWidthAsString = rr.getVariable("WIDTH");
                robotHeightAsString = rr.getVariable("HEIGHT");
                robotRasString = rr.getVariable("MEDIAN_COLOR_R");
                robotGasString = rr.getVariable("MEDIAN_COLOR_G");
                robotBasString = rr.getVariable("MEDIAN_COLOR_B");

                //parse the data into an array
                robotCOGXAsStringArray = new List<string>(robotCOGXAsString.Split(','));
                robotCOGYAsStringArray = new List<string>(robotCOGYAsString.Split(','));
                robotWidthAsStringArray = new List<string>(robotWidthAsString.Split(','));
                robotHeightAsStringArray = new List<string>(robotHeightAsString.Split(','));
                robotRasStringArray = new List<string>(robotRasString.Split(','));
                robotGasStringArray = new List<string>(robotGasString.Split(','));
                robotBasStringArray = new List<string>(robotBasString.Split(','));

                //Create new Drones in a seperate list. We use the list to compare to the current robots list. We check for changes
                for (int i = 0; i < robotCOGXAsStringArray.Count; i++)
                {
                    Drones rbt = new Drones();

                    rbt.cog_x = (int) Double.Parse(robotCOGXAsStringArray[i]);
                    rbt.cog_y = (int) Double.Parse(robotCOGYAsStringArray[i]);
                    rbt.width = (int) Double.Parse(robotWidthAsStringArray[i]);
                    rbt.height = (int) Double.Parse(robotHeightAsStringArray[i]);
                    rbt.R = (int) Double.Parse(robotRasStringArray[i]);
                    rbt.G = (int) Double.Parse(robotGasStringArray[i]);
                    rbt.B = (int) Double.Parse(robotBasStringArray[i]);
                    checkListForRRDrones.Add(rbt);
                }

                //Time to check for changes, loop through our drones from roborealm and see if they already exist in robots list
                //If we find a chance update our flag
                foreach (Drones drone in checkListForRRDrones)
                {
                    if (!robots.Contains(drone))
                        dirtyFlagForRobotsList = true;
                }


                //If the flag has been thrown clear the robots array and populate it with the new data
                if (dirtyFlagForRobotsList)
                {
                    robots.Clear();
                    foreach (Drones drone in checkListForRRDrones)
                    {
                        robots.Add(drone);
                    }

                    dirtyFlagForRobotsList = false;


                    //Check to see if there are any subscribers, If there are call the event
                    if(dronesCollectionModified != null)
                        dronesCollectionModified(robots);
                }
                
            }

            //Sometimes the data comes in wrong from the socket, write out the error
            //Needs to be updated to write to the rx/tx window as status instead of writing to the console
            catch (Exception exception) {
                Console.WriteLine(exception.Message);
            }
           
        }

        //Property for the width of the roborealm resolution
        //Returns 0 if we haven't gotten info from RR yet
        public int width
        {
            get
            {
                if(dimension != null)
                    return dimension.width;
                return 0;
            }
        }

        //Property for the height of the roborealm resolution
        //Returns 0 if we haven't gotten info from RR yet
        public int height
        {
            get
            {
                if(dimension != null)
                    return dimension.height;
                return 0;
            }
        }

    }    
}
